home *** CD-ROM | disk | FTP | other *** search
/ Programmer Power Tools / Programmer Power Tools.iso / math / praxis.arc / MINFIT.C < prev    next >
C/C++ Source or Header  |  1987-07-22  |  4KB  |  178 lines
  1. #include <stdio.h>
  2. #include <math.h>
  3. #include "machine.h"
  4.  
  5. #ifdef MSDOS
  6. static double e[N];    /* to save stack space */
  7. #endif
  8.  
  9. /* singular value decomposition */
  10.  
  11. minfit(n, eps, tol, ab, q)
  12. int n;
  13. double eps, tol, ab[N][N], q[N];
  14. {
  15.    int l, kt, l2, i, j, k;
  16.    double c, f, g, h, s, x, y, z;
  17. #ifndef MSDOS
  18.    double e[N];        /* plenty of stack on a vax */
  19. #endif
  20.  
  21.    /* householder's reduction to bidiagonal form */
  22.    x = g = 0.0;
  23.    for (i=0; i<n; i++) {
  24.        e[i] = g; s = 0.0; l = i+1;
  25.        for (j=i; j<n; j++)
  26.        s += ab[j][i] * ab[j][i];
  27.        if (s < tol) {
  28.       g = 0.0;
  29.        }
  30.        else {
  31.       f = ab[i][i];
  32.           if (f < 0.0) 
  33.          g = sqrt(s);
  34.       else
  35.          g = -sqrt(s);
  36.       h = f*g - s; ab[i][i] = f - g;
  37.       for (j=l; j<n; j++) {
  38.           f = 0.0;
  39.           for (k=i; k<n; k++)
  40.           f += ab[k][i] * ab[k][j];
  41.           f /= h;
  42.           for (k=i; k<n; k++)
  43.           ab[k][j] += f * ab[k][i];
  44.       }
  45.        }
  46.        q[i] = g; s = 0.0;
  47.        if (i < n)
  48.       for (j=l; j<n; j++)
  49.           s += ab[i][j] * ab[i][j];
  50.        if (s < tol) {
  51.       g = 0.0;
  52.        }
  53.        else {
  54.       f = ab[i][i+1];
  55.       if (f < 0.0)
  56.          g = sqrt(s);
  57.       else 
  58.          g = - sqrt(s);
  59.       h = f*g - s; ab[i][i+1] = f - g;
  60.       for (j=l; j<n; j++)
  61.           e[j] = ab[i][j]/h;
  62.       for (j=l; j<n; j++) {
  63.           s = 0;
  64.           for (k=l; k<n; k++) s += ab[j][k]*ab[i][k];
  65.           for (k=l; k<n; k++) ab[j][k] += s * e[k];
  66.       }
  67.        }
  68.        y = fabs(q[i]) + fabs(e[i]);
  69.        if (y > x) x = y;
  70.    }
  71.    /* accumulation of right hand transformations */
  72.    for (i=n-1; i >= 0; i--) {
  73.        if (g != 0.0) {
  74.           h = ab[i][i+1]*g;
  75.       for (j=l; j<n; j++) ab[j][i] = ab[i][j] / h;
  76.       for (j=l; j<n; j++) {
  77.               s = 0.0;
  78.           for (k=l; k<n; k++) s += ab[i][k] * ab[k][j];
  79.           for (k=l; k<n; k++) ab[k][j] += s * ab[k][i];
  80.       }
  81.        }
  82.        for (j=l; j<n; j++)
  83.            ab[i][j] = ab[j][i] = 0.0;
  84.        ab[i][i] = 1.0; g = e[i]; l = i;
  85.    }
  86.    /* diagonalization to bidiagonal form */
  87.    eps *= x;
  88.    for (k=n-1; k>= 0; k--) {
  89.        kt = 0;
  90. TestFsplitting:
  91.        if (++kt > 30) {
  92.           e[k] = 0.0;
  93.       fprintf(stderr, "\n+++ qr failed\n");
  94.        }
  95.        for (l2=k; l2>=0; l2--) {
  96.            l = l2;
  97.        if (fabs(e[l]) <= eps)
  98.           goto TestFconvergence;
  99.        if (fabs(q[l-1]) <= eps)
  100.              break;    /* goto Cancellation; */
  101.        }
  102. Cancellation:
  103.        c = 0.0; s = 1.0;
  104.        for (i=l; i<=k; i++) {
  105.            f = s * e[i]; e[i] *= c;
  106.        if (fabs(f) <= eps)
  107.           goto TestFconvergence;
  108.        g = q[i];
  109.           if (fabs(f) < fabs(g)) {
  110.           double fg = f/g;
  111.           h = fabs(g)*sqrt(1.0+fg*fg);
  112.        }
  113.        else {
  114.           double gf = g/f;
  115.           h = (f!=0.0 ? fabs(f)*sqrt(1.0+gf*gf) : 0.0);
  116.        }
  117.        q[i] = h;
  118.        if (h == 0.0) { h = 1.0; g = 1.0; }
  119.        c = g/h; s = -f/h;
  120.        }
  121. TestFconvergence:
  122.        z = q[k];
  123.        if (l == k)
  124.           goto Convergence;
  125.        /* shift from bottom 2x2 minor */
  126.        x = q[l]; y = q[k-l]; g = e[k-1]; h = e[k];
  127.        f = ((y-z)*(y+z) + (g-h)*(g+h)) / (2.0*h*y);
  128.        g = sqrt(f*f+1.0);
  129.        if (f <= 0.0)
  130.           f = ((x-z)*(x+z) + h*(y/(f-g)-h))/x;
  131.        else
  132.           f = ((x-z)*(x+z) + h*(y/(f+g)-h))/x;
  133.        /* next qr transformation */
  134.        s = c = 1.0;
  135.        for (i=l+1; i<=k; i++) {
  136.            g = e[i]; y = q[i]; h = s*g; g *= c;
  137.        if (fabs(f) < fabs(h)) {
  138.           double fh = f/h;
  139.           z = fabs(h) * sqrt(1.0 + fh*fh);
  140.        }
  141.        else {
  142.           double hf = h/f;
  143.           z = (f!=0.0 ? fabs(f)*sqrt(1.0+hf*hf) : 0.0);
  144.        }
  145.        e[i-1] = z;
  146.        if (z == 0.0) 
  147.            f = z = 1.0;
  148.        c = f/z; s = h/z;
  149.        f = x*c + g*s; g = - x*s + g*c; h = y*s;
  150.        y *= c;
  151.        for (j=0; j<n; j++) {
  152.            x = ab[j][i-1]; z = ab[j][i];
  153.            ab[j][i-1] = x*c + z*s;
  154.            ab[j][i] = - x*s + z*c;
  155.        }
  156.        if (fabs(f) < fabs(h)) {
  157.           double fh = f/h;
  158.           z = fabs(h) * sqrt(1.0 + fh*fh);
  159.        }
  160.        else {
  161.           double hf = h/f;
  162.           z = (f!=0.0 ? fabs(f)*sqrt(1.0+hf*hf) : 0.0);
  163.        }
  164.            q[i-1] = z;
  165.        if (z == 0.0) z = f = 1.0;
  166.        c = f/z; s = h/z;
  167.        f = c*g + s*y; x = - s*g + c*y;
  168.        }
  169.        e[l] = 0.0; e[k] = f; q[k] = x;
  170.        goto TestFsplitting;
  171. Convergence:
  172.        if (z < 0.0) {
  173.           q[k] = - z;
  174.       for (j=0; j<n; j++) ab[j][k] = - ab[j][k];
  175.        }
  176.    }
  177. }
  178.